/*
      Copyright (C) 2010 Bepu Entertainment LLC.

      This software source code is provided 'as-is', without 
      any express or implied warranty.  In no event will the authors be held 
      liable for any damages arising from the use of this software.

      Permission is granted to anyone to use this software for any purpose,
      including commercial applications, and to alter it and redistribute it
      freely, subject to the following restrictions:

      1. The origin of this software must not be misrepresented; you must not
         claim that you wrote the original software. If you use this software
         in a product, an acknowledgment in the product documentation would be
         appreciated but is not required.
      2. Altered source versions must be plainly marked as such, and must not be
         misrepresented as being the original software.
      3. This notice may not be removed or altered from any source distribution.

    Contact us at:
    contact@bepu-games.com
 */

using BEPUphysics;
using BEPUphysics.Entities;
using Microsoft.Xna.Framework;

namespace BEPUphysicsDemos.SampleCode
{
    /// <summary>
    /// Grabs an entity at a specified location and applies corrective impulses to keep the grabbed location near the goal location.
    /// </summary>
    public class GrabSpring : Updateable
    {
        /*
         * This class is a cleaned up version of the GrabSpring present in v0.11.0 and previous versions.
         * It's been moved into the demos as an example (and to clean up the main library).
         * 
         */

        /// <summary>
        /// Offset from the center of the entity to the grabbed location in local space.
        /// </summary>
        private Vector3 localOffset;


        /// <summary>
        /// Constructs a grab constraint.
        /// </summary>
        /// <param name="correctiveStrength">Factor of the position error to use in corrective impulses each frame.</param>
        /// <param name="linearDamp">Damping to apply to the grabbed entity's linear momentum.</param>
        /// <param name="angularDamp">Damping to apply to the grabbed entity's angular momentum.</param>
        public GrabSpring(float correctiveStrength, float linearDamp, float angularDamp)
        {
            CorrectionFactor = correctiveStrength;
            LinearDamping = linearDamp;
            AngularDamping = angularDamp;
        }

        /// <summary>
        /// Gets or sets damping to apply to the grabbed entity's angular momentum.
        /// </summary>
        public float AngularDamping { get; set; }

        /// <summary>
        /// Gets or sets the factor of the position error to use in corrective impulses each frame.
        /// </summary>
        public float CorrectionFactor { get; set; }

        /// <summary>
        /// Gets the grabbed entity.
        /// </summary>
        public Entity Entity { get; private set; }

        /// <summary>
        /// Gets the location that the entity will be pulled towards.
        /// </summary>
        public Vector3 GoalPosition { get; set; }

        /// <summary>
        /// Gets the last updated position of the grab location on the surface of the entity.
        /// </summary>
        public Vector3 GrabbedPosition { get; private set; }

        /// <summary>
        /// Getes or sets damping to apply to the grabbed entity's linear momentum.
        /// </summary>
        public float LinearDamping { get; set; }


        /// <summary>
        /// Updates the grab constraint's grab position after the end of a frame.
        /// </summary>
        /// <param name="dt">Time since last frame in simulation seconds.</param>
        public override void UpdateAtEndOfFrame(float dt)
        {
            //Since the grabbed position is usually examined graphically, 
            //it's good to use the interpolated positions in case the 
            //engine is using internal time stepping and interpolation.
            GrabbedPosition = Vector3.TransformNormal(localOffset, Entity.OrientationMatrix) + Entity.CenterOfMass;
        }

        /// <summary>
        /// Applies an appropriate impulse to correct the position of the entity.
        /// Called automatically by the space.
        /// </summary>
        /// <param name="dt">Time since last frame in simulation seconds.</param>
        public override void UpdateDuringForces(float dt)
        {
            if (Entity.IsDynamic)
            {
                Vector3 grabbedLocation = Vector3.TransformNormal(localOffset, Entity.InternalOrientationMatrix) + Entity.InternalCenterOfMass;
                Vector3 positionError = GoalPosition - grabbedLocation;

                //Apply the force.
                Entity.ApplyImpulse(grabbedLocation, positionError * CorrectionFactor * dt * Entity.Mass);

                //Entities provide a little helper for damping.
                //This isn't exactly 'physically correct,' but
                //for what the grab spring is used for, it works fine.
                Entity.ModifyLinearDamping(LinearDamping);
                Entity.ModifyAngularDamping(AngularDamping);
            }
        }

        /// <summary>
        /// Reinitializes the grabbing constraint with new information.
        /// </summary>
        /// <param name="e">Entity to grab.</param>
        /// <param name="grabLocation">Location on the entity being grabbed in world space.</param>
        public void Setup(Entity e, Vector3 grabLocation)
        {
            Entity = e;
            localOffset = Vector3.Transform(grabLocation - e.InternalCenterOfMass, Quaternion.Conjugate(e.InternalOrientationQuaternion));
            GoalPosition = grabLocation;
        }
    }
}